/**********************************************
*  Namespace: AI
*  Classes:   SteeringAlign
*  Desc:      Updates positions using a Align
*             algorithm in 2d or 3d cases
*  Author:    Vaios Kalpias-Ilias (C) 2008-2009
***********************************************/

#include "SteeringAlign.h"
#include "Helper.h"

namespace AI
{
	//---------------------------------------------
	/**
	* Ctor
	*/
	SteeringAlign::SteeringAlign()
	{
	}
	//---------------------------------------------
	/**
	* Dtor
	*/
	SteeringAlign::~SteeringAlign()
	{
	}
	//---------------------------------------------
	/**
	* Returns new kinematic data in 2d
	* @param current
	* @param target
	*/
	Steering2d SteeringAlign::GetSteering(const Kinematic2d& current,
									   const Kinematic2d& target)
	{		
		// get direction to target
		float rotation = target.orientation - current.orientation;
		
		// clamp result to -pi, pi
		rotation = mth::Helper::ClampAngle(rotation,
			-mth::M_PI,mth::M_PI);

		float rotationSize = abs(rotation);
		// if we are there return no steering
		Steering2d steering;
		if (rotationSize < rotationTargetRadius)
		{
			return steering;
		}

		// if we are outside the slow radius then rotate
		// at max speed
		float targetRotation;
		if (rotationSize > rotationSlowRadius)
		{
			targetRotation = maxRotation;
		}
		// else calculate scaled rotation
		else
		{
			targetRotation = maxRotation * rotationSize / rotationSlowRadius;
		}

		// final rotation combines speed and direction
		targetRotation *= rotation / rotationSize;

		// acceleration tries to get to target rotation
		steering.angular = (targetRotation - current.rotation) / rotationTimeToTarget;

		// trim acceleration
		float angularAcceleration = abs(steering.angular);
		if (angularAcceleration > maxAngularAcceleration)
		{
			steering.angular /= angularAcceleration;
			steering.angular *= maxAngularAcceleration;
		}

		return steering;
	}

}
